import RPi.GPIO as GPIO
import time
from pi.model.log import log

"""
A: 左边
B：右边

IN 1和IN 2取值
    0,1: 前进
    1,0: 后退
    0,0: 停止
    1,1: 

运动情况有前、后、左、右、前左、前右、后左、后右八种:
    前后：直线前/后
    左右：原地转弯
    前左/前右/后左后右：保持前/后的情况下，将左右侧的速度调整不同
"""


class moveDao:
    # todo：这里的引脚要用confDao动态获取
    # AIN1,AIN2,AIN3,AIN4 =
    PWMA = 18
    AIN1 = 22
    AIN2 = 27

    PWMB = 23
    BIN1 = 25
    BIN2 = 24

    logger = log("./dao/moveDao/")  # 输出log

    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(AIN2, GPIO.OUT)
    GPIO.setup(AIN1, GPIO.OUT)
    GPIO.setup(PWMA, GPIO.OUT)

    GPIO.setup(BIN1, GPIO.OUT)
    GPIO.setup(BIN2, GPIO.OUT)
    GPIO.setup(PWMB, GPIO.OUT)

    L_Motor = GPIO.PWM(PWMA, 100)
    L_Motor.start(0)

    R_Motor = GPIO.PWM(PWMB, 100)
    R_Motor.start(0)

    def _t_move(self, l_speed: int, r_speed: int, t_time):
        """
        控制左右两边的速度和方向
        :param l_speed: 左边的速度,负数代表反向
        :param r_speed: 右边的速度,负数代表反向
        :param t_time: 运动时间，默认运动结束之后停止，-1代表运动之后保持状态
        :return: 无
        """
        if l_speed > 0:
            self.L_Motor.ChangeDutyCycle(l_speed)
            GPIO.output(self.AIN2, False)  # AIN2
            GPIO.output(self.AIN1, True)  # AIN1
        elif l_speed < 0:
            self.L_Motor.ChangeDutyCycle(-1 * l_speed)
            GPIO.output(self.AIN2, True)  # AIN2
            GPIO.output(self.AIN1, False)  # AIN1
        else:
            self.L_Motor.ChangeDutyCycle(l_speed)
            GPIO.output(self.AIN2, False)  # AIN2
            GPIO.output(self.AIN1, False)  # AIN1

        if r_speed > 0:
            self.R_Motor.ChangeDutyCycle(r_speed)
            GPIO.output(self.BIN2, False)  # BIN2
            GPIO.output(self.BIN1, True)  # BIN1
        elif r_speed < 0:
            self.R_Motor.ChangeDutyCycle(-1 * r_speed)
            GPIO.output(self.BIN2, True)  # BIN2
            GPIO.output(self.BIN1, False)  # BIN1
        else:
            self.R_Motor.ChangeDutyCycle(0)
            GPIO.output(self.BIN2, False)  # BIN2
            GPIO.output(self.BIN1, False)  # BIN1

        if t_time == -1:
            return
        else:
            time.sleep(t_time)
            self.R_Motor.ChangeDutyCycle(0)
            GPIO.output(self.BIN2, False)  # BIN2
            GPIO.output(self.BIN1, False)  # BIN1
        # print(l_speed, r_speed, t_time)
        self.logger.debug(str(l_speed) + "_" + str(r_speed) + "_" + str(t_time))

    def t_up(self, speed: int = 50, t_time: int = -1):
        self._t_move(speed, speed, t_time)


    def t_stop(self, t_time: int = -1):
        self._t_move(0, 0, t_time)

    def t_down(self, speed: int = 50, t_time: int = -1):
        self._t_move(-1 * speed, -1 * speed, t_time)

    def t_left_local(self, speed: int = 50, t_time: int = -1):
        """
        原地左转
        :param speed:
        :param t_time:
        :return:
        """
        self._t_move(-1 * speed, speed, t_time)

    def t_right_local(self, speed: int = 50, t_time: int = -1):
        """
        原地右转
        :param speed:
        :param t_time:
        :return:
        """
        self._t_move(speed, -1 * speed, t_time)

    def t_left(self, speed: int = 50, t_time: int = -1, diff_speed: int = 20):
        """
        左前方转弯，左边减速，右边加速
        :param speed: 转弯整体速度
        :param t_time: 执行时间
        :param diff_speed: 左右差速
        :return:
        """
        if speed < diff_speed:
            if speed >= 0:
                self._t_move(0, speed + diff_speed, t_time)
            else:  # 倒车
                self._t_move(speed - diff_speed, speed + diff_speed, t_time)
        else:
            self._t_move(speed - diff_speed, speed + diff_speed, t_time)

    def t_right(self, speed: int = 50, t_time: int = -1, diff_speed: int = 20):
        """
        右前方转弯，左边加速，右边减速
        :param speed: 转弯整体速度
        :param t_time: 执行时间
        :param diff_speed: 左右差速
        :return:
        """
        if speed < diff_speed:
            if speed >= 0:
                self._t_move(speed + diff_speed, 0, t_time)
            else:  # 倒车
                self._t_move(speed + diff_speed, speed - diff_speed, t_time)
        else:
            self._t_move(speed + diff_speed, speed - diff_speed, t_time)
